#ifndef __MC_TRAJECTORY_ROS_H__
#define __MC_TRAJECTORY_ROS_H__

#include "trajectory_2d.h"
#include <geometry_msgs/Pose.h>                     // 用于toROSPose
#include <geometry_msgs/PoseStamped.h>              // 用于toROSPoseStamped
#include <nav_msgs/Path.h>                          // 用于toROSPath

namespace mc{

class TrajectoryROS{
    public:
        // 轨迹长度
        double calculateLength(Trajectory2D& trajectory) const;
        // 计算轨迹曲率
        void calculateCurvature(Trajectory2D& trajectory);

        // 计算两轨迹点距离
        double calculateDistance(TrajectoryPoint2D& a, TrajectoryPoint2D& b) const;
        // 计算两轨迹点距离平方
        double calculateDistanceSquared(TrajectoryPoint2D& a, TrajectoryPoint2D& b) const;
        // 计算轨迹点a相对于轨迹点b的相对坐标
        TrajectoryPoint2D calculateRelativeCoordinate(TrajectoryPoint2D& a, TrajectoryPoint2D& b) const;

        // Pose2D <-> geometry_msgs::Pose
        Pose2D toPose2D(geometry_msgs::Pose& msg) const;
        geometry_msgs::Pose toROSPose(Pose2D& pose) const;

        // Pose2D <-> geometry_msgs::PoseStamped
        Pose2D toPose2D(geometry_msgs::PoseStamped& msg) const;
        geometry_msgs::PoseStamped toROSPoseStamped(Pose2D& pose, string frame_id) const;

        // TrajectoryPoint2D <-> geometry_msgs::Pose
        TrajectoryPoint2D toTrajectoryPoint2D(geometry_msgs::Pose& msg, string frame_id) const;
        geometry_msgs::Pose toROSPose(TrajectoryPoint2D& pt) const;

        // TrajectoryPoint2D <-> geometry_msgs::PoseStamped
        TrajectoryPoint2D toTrajectoryPoint2D(geometry_msgs::PoseStamped& msg) const;
        geometry_msgs::PoseStamped toROSPoseStamped(TrajectoryPoint2D& pt) const;
        
        // Trajectory2D <-> nav_msgs::Path
        Trajectory2D toTrajectory2D(nav_msgs::Path& path) const;
        nav_msgs::Path toROSPath(Trajectory2D& trajectory, string frame_id) const;
    
};

};  // namespace mc

#endif